#include "can_task.h"
#include "can_moto_protocol.h"

static uint16_t LocalCanID = 0;
static xQueueHandle CanRxMsgQ;
TypeDefBspCanStruct can_handle;
static void can_rx_callback(uint16_t ID,uint8_t *Dat,uint8_t Len);
static void (*protocol_rx)(uint16_t ID,uint8_t *Dat,uint8_t Len);
/*********************************************************************************************
  * @brief  MotorsTask  电机任务
  * @param  void *pvParameters 任务参数指针
  * @retval None
	* @IO None
  ********************************************************************************************/
void CanTask(void *pvParameters)
{
//	can_msg_t can_msg;
//	BaseType_t QueueStatus;
//	BspDeviceID_IoConfig();
//	LocalCanID = BspDeviceID_ReadID();
//	can_handle.NodeID = LocalCanID;
//	can_handle.CanRx0_Handle = can_rx_callback;
//	can_handle.CanRx1_Handle = can_rx_callback;
//	BspCan1_Init(&can_handle);
//	CanRxMsgQ = xQueueCreate(10,sizeof(can_msg_t));
//	if(CanRxMsgQ == NULL)
//		while(1);
//	
//	can_msg_t nmt_msg;
//	can_nmt_sw(can_handle.NodeID,NMT_STATUS_ONLINE,&nmt_msg);
//	Can1SendData(nmt_msg.can_id,nmt_msg.can_data,nmt_msg.can_dlc);
	while(1)
	{
//		QueueStatus = xQueueReceive(CanRxMsgQ,&can_msg,1);
//		if(QueueStatus == pdPASS)
//		{
//      	if(protocol_rx != NULL)
//				protocol_rx(can_msg.can_id,can_msg.can_data,can_msg.can_dlc);
//		}
		vTaskDelay(1);
	}
}
void Can_init(void)
{
	can_msg_t can_msg;
	BaseType_t QueueStatus;
	BspDeviceID_IoConfig();
	LocalCanID = BspDeviceID_ReadID();
	can_handle.NodeID = LocalCanID;
	can_handle.CanRx0_Handle = can_rx_callback;
	can_handle.CanRx1_Handle = can_rx_callback;
	BspCan1_Init(&can_handle);
	CanRxMsgQ = xQueueCreate(10,sizeof(can_msg_t));
	if(CanRxMsgQ == NULL)
		while(1);
	
	can_msg_t nmt_msg;
	can_nmt_sw(can_handle.NodeID,NMT_STATUS_ONLINE,&nmt_msg);
	Can1SendData(nmt_msg.can_id,nmt_msg.can_data,nmt_msg.can_dlc);
}
void Can_poll(void)
{
	can_msg_t can_msg;
	BaseType_t QueueStatus;
	QueueStatus = xQueueReceive(CanRxMsgQ,&can_msg,1);
    if(QueueStatus == pdPASS)
	{
		if(protocol_rx != NULL)
			protocol_rx(can_msg.can_id,can_msg.can_data,can_msg.can_dlc);
	}
}

static void can_rx_callback(uint16_t ID,uint8_t *Dat,uint8_t Len)
{
	can_msg_t CanMsg;
	static portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
	CanMsg.can_id = ID;
	CanMsg.can_dlc = Len;
	memcpy(CanMsg.can_data,Dat,Len);
	xQueueSendFromISR(CanRxMsgQ, &CanMsg, &xHigherPriorityTaskWoken);
  portEND_SWITCHING_ISR(xHigherPriorityTaskWoken);
}
void CanProtoReg(void (*pfn)(uint16_t ID,uint8_t *Dat,uint8_t Len))
{
	protocol_rx = pfn;
}